import serial
from struct import pack, unpack
uart = serial.Serial("/dev/ttyS4", 115200)
def sendcmd(cmd):
    sumA = 0
    sumB = 0
    data = bytearray([0x41,0x43])
    uart.write(data)
    data = bytearray([cmd,8])
    float_value = 0
    float_bytes = pack('f', float_value)
    for b in float_bytes:
        sumB = sumB + b
        sumA = sumA + sumB

    uart.write(float_bytes)
    float_value = 0
    float_bytes = pack('f', float_value)
    for b in float_bytes:
        sumB = sumB + b
        sumA = sumA + sumB

    uart.write(float_bytes)
    sumB%=256
    sumA%=256
    data = bytearray([sumB, sumA])
    uart.write(data)
    # print(cmd)

def sendTarget(target=(None,None)):
    if(target[0]!=None):
        sumA = 0
        sumB = 0
        data = bytearray([0x41,0x43])
        uart.write(data)
        # cx=0
        # cy=0
        # bx_x=cx-160
        # by_y=120-cy
        bx_x=target[0]*1.5
        by_y=target[1]*1.5
        user_defined=-1
        data = bytearray([0x02,12])
        for b in data:
            sumB = sumB + b
            sumA = sumA + sumB
        # print(data)
        uart.write(data)

        float_value = bx_x
        float_bytes = pack('f', float_value)
        for b in float_bytes:
            sumB = sumB + b
            sumA = sumA + sumB
        # print(float_bytes)
        uart.write(float_bytes)

        float_value = by_y
        # float_value = 0
        float_bytes = pack('f', float_value)
        for b in float_bytes:
            sumB = sumB + b
            sumA = sumA + sumB
        # print(float_bytes)
        uart.write(float_bytes)

        float_value = user_defined
        float_bytes = pack('f', float_value)
        for b in float_bytes:
            sumB = sumB + b
            sumA = sumA + sumB
        # print(float_bytes)
        uart.write(float_bytes)
        
        # print(sumB,sumA)
        sumB%=256
        sumA%=256
        data = bytearray([sumB, sumA])
        uart.write(data)
    else:
        sumA = 0
        sumB = 0

        data = bytearray([0x41,0x43])
        uart.write(data)

        data = bytearray([0x01,0])
        for b in data:
            sumB = sumB + b
            sumA = sumA + sumB
        uart.write(data)

        data = bytearray([sumB%256, sumA%256])
        uart.write(data)
        print("none")
    

# while(True):
#     sumA = 0
#     sumB = 0
#     data = bytearray([0x41,0x43])
#     uart.write(data)
#     # cx=0
#     # cy=0
#     # bx_x=cx-160
#     # by_y=120-cy
#     bx_x=10.5
#     by_y=25.3
#     user_defined=-1
#     data = bytearray([0x02,12])
#     for b in data:
#         sumB = sumB + b
#         sumA = sumA + sumB
#     print(data)
#     uart.write(data)

#     float_value = bx_x
#     float_bytes = pack('f', float_value)
#     for b in float_bytes:
#         sumB = sumB + b
#         sumA = sumA + sumB
#     print(float_bytes)
#     uart.write(float_bytes)

#     float_value = by_y
#     float_bytes = pack('f', float_value)
#     for b in float_bytes:
#         sumB = sumB + b
#         sumA = sumA + sumB
#     print(float_bytes)
#     uart.write(float_bytes)

#     float_value = user_defined
#     float_bytes = pack('f', float_value)
#     for b in float_bytes:
#         sumB = sumB + b
#         sumA = sumA + sumB
#     print(float_bytes)
#     uart.write(float_bytes)
    
#     print(sumB,sumA)
#     sumB%=256
#     sumA%=256
#     data = bytearray([sumB, sumA])
#     uart.write(data)

#     print("found: x=",bx_x,"  y=",by_y)
#     #b_output_str="x%d%d,y%d%d" % (bx_value,bx_x,by_value,by_y)
#     #print('you send black:',b_output_str)
    #uart.write(b_output_str+'\r\n')
# while(True):
#     sendTarget((0,-200))
#     print((0,10))

#(+,_)左
#(-,_)右
#(_,+)前
#(_,-)后